#include <rclcpp/rclcpp.hpp>
#include <nav_msgs/msg/path.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <geometry_msgs/msg/twist.hpp>

#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
#include <tf2/utils.h>

#include "interface/controller.h"
#include "interface/common.h"
#include "smooth.h"
class PathFollower : public rclcpp::Node {
public:
    PathFollower() : Node("path_follower"), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_) {

        controller_ = ControllerInterface::create();

        // 订阅全局规划器规划的原始路径
        path_subscription_ = create_subscription<nav_msgs::msg::Path>(
            "/plan", 10, std::bind(&PathFollower::PathCallback, this, std::placeholders::_1));

        odometry_subscription_ = create_subscription<nav_msgs::msg::Odometry>(
            "/odom", 10, std::bind(&PathFollower::OdometryCallback, this, std::placeholders::_1));

        // 创建发布者，将控制算法返回的线速度和角速度发布到/cmd_vel话题，用于控制小车运动
        cmd_vel_publisher_ = this->create_publisher<geometry_msgs::msg::Twist>("/cmd_vel", 10);

        // 发布降采样的路径，用于可视化
        ds_path_publisher_ = this->create_publisher<nav_msgs::msg::Path>("/ds_plan", 10);

        // 发布平滑后的路径， 用于可视化
        smooth_path_publisher_ = this->create_publisher<nav_msgs::msg::Path>("/smooth_plan", 10);

        // 定时器，用于设置控制周期
        control_timer_ =
            this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&PathFollower::ControlLoop, this));
    }

private:
    void ControlLoop() {
        float linear_vel, steer_angle;
        controller_->getOut(linear_vel, steer_angle);
        geometry_msgs::msg::Twist cmd_vel;
        FollowState               state = controller_->getState();
        std::cout << "state:" << state._to_string() << std::endl;
        if (state == +FollowState::kFollowing) {
            cmd_vel.linear.x  = linear_vel;
            cmd_vel.angular.z = steer_angle;
        } else {
            cmd_vel.linear.x  = 0;
            cmd_vel.angular.z = 0;
        }

        cmd_vel_publisher_->publish(cmd_vel);
    }

    void OdometryCallback(const nav_msgs::msg::Odometry::SharedPtr odometry_msg) {
        // 更新控制器的位姿
        std::vector<Odometry> poses(2);

        Odometry pose;
        pose.x   = odometry_msg->pose.pose.position.x;
        pose.y   = odometry_msg->pose.pose.position.y;
        pose.yaw = tf2::getYaw(odometry_msg->pose.pose.orientation);
        pose.v   = std::hypot(odometry_msg->twist.twist.linear.x, odometry_msg->twist.twist.linear.y);
        pose.w   = odometry_msg->twist.twist.angular.z;
        poses[0] = pose;

        // 获取转向轮的位姿
        try {
            geometry_msgs::msg::TransformStamped transform_stamped =
                tf_buffer_.lookupTransform("map", "steer_wheel", tf2::TimePointZero);
            pose.x   = transform_stamped.transform.translation.x;
            pose.y   = transform_stamped.transform.translation.y;
            pose.yaw = tf2::getYaw(transform_stamped.transform.rotation);
        } catch (tf2::TransformException &ex) {
            RCLCPP_WARN(this->get_logger(), "Could not transform %s to %s: %s", "base_link", "map", ex.what());
        }
        poses[1] = pose;

        controller_->update(poses);
    }

    void PathCallback(const nav_msgs::msg::Path::SharedPtr path_msg) {
        if (path_msg->poses.empty()) {
            RCLCPP_WARN(this->get_logger(), "Received empty path");
            return;
        }

        RCLCPP_INFO(this->get_logger(), "New path received with %zu points", path_msg->poses.size());

        // 添加路径点的坐标
        Path path;
        for (const auto &pose : path_msg->poses) {
            Point point;
            point.x = pose.pose.position.x;
            point.y = pose.pose.position.y;
            path.emplace_back(point);
        }

        auto ds_path     = douglasPeucker(path, 0.6);
        auto smooth_path = smoothPath(ds_path, 0.25, 30);
        PathProcess(smooth_path);

        controller_->startFollow(smooth_path);

        /** 可视化 **/
        // 发送降采样的路径
        nav_msgs::msg::Path ds_path_msg;
        ds_path_msg.header.frame_id = "map";
        ds_path_msg.header.stamp    = this->get_clock()->now();
        for (const auto &point : ds_path) {
            geometry_msgs::msg::PoseStamped pose_stamped;
            pose_stamped.header.frame_id    = "map";
            pose_stamped.pose.position.x    = point.x;
            pose_stamped.pose.position.y    = point.y;
            pose_stamped.pose.orientation.x = 0;
            pose_stamped.pose.orientation.y = 0;
            pose_stamped.pose.orientation.z = sin(point.yaw / 2);
            pose_stamped.pose.orientation.w = cos(point.yaw / 2);
            ds_path_msg.poses.emplace_back(pose_stamped);
        }
        ds_path_publisher_->publish(ds_path_msg);

        // 发送平滑后的路径
        nav_msgs::msg::Path smooth_path_msg;
        smooth_path_msg.header.frame_id = "map";
        smooth_path_msg.header.stamp    = this->get_clock()->now();
        for (const auto &point : smooth_path) {
            geometry_msgs::msg::PoseStamped pose_stamped;
            pose_stamped.header.frame_id    = "map";
            pose_stamped.pose.position.x    = point.x;
            pose_stamped.pose.position.y    = point.y;
            pose_stamped.pose.orientation.x = 0;
            pose_stamped.pose.orientation.y = 0;
            pose_stamped.pose.orientation.z = sin(point.yaw / 2);
            pose_stamped.pose.orientation.w = cos(point.yaw / 2);
            smooth_path_msg.poses.emplace_back(pose_stamped);
        }

        smooth_path_publisher_->publish(smooth_path_msg);
    }

    void PathProcess(std::vector<Point> &path) {
        if (path.size() < 3) {
            std::cout << "set path error, path size is too small!" << std::endl;
            return;
        }

        // 计算每个点的方向
        // 处理第一个点
        double dx   = path[1].x - path[0].x;
        double dy   = path[1].y - path[0].y;
        path[0].yaw = std::atan2(dy, dx);

        // 处理最后一个点
        size_t last    = path.size() - 1;
        dx             = path[last].x - path[last - 1].x;
        dy             = path[last].y - path[last - 1].y;
        path[last].yaw = std::atan2(dy, dx);

        // 计算中间点
        for (size_t i = 1; i < path.size() - 1; ++i) {
            double dx   = 0.5 * (path[i + 1].x - path[i - 1].x);
            double dy   = 0.5 * (path[i + 1].y - path[i - 1].y);
            path[i].yaw = std::atan2(dy, dx);
        }

        // 计算每个点的曲率
        // 计算三点构成的圆的曲率
        auto calculateCurvature = [](const Point &p1, const Point &p2, const Point &p3) -> double {
            double a    = std::sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y));
            double b    = std::sqrt((p2.x - p3.x) * (p2.x - p3.x) + (p2.y - p3.y) * (p2.y - p3.y));
            double c    = std::sqrt((p3.x - p1.x) * (p3.x - p1.x) + (p3.y - p1.y) * (p3.y - p1.y));
            double s    = (a + b + c) / 2;
            double area = std::sqrt(s * (s - a) * (s - b) * (s - c));

            // 检查是否能构成三角形
            if (a + b <= c || a + c <= b || b + c <= a)
                return 0.0;

            return 4 * area / (a * b * c);
        };

        for (int i = 0; i < path.size() - 2; ++i) {
            float curvature   = calculateCurvature(path[i], path[i + 1], path[i + 2]);
            path[i].curvature = curvature;
        }

        // 最后两个点的曲率可以设为和倒数第三个点相同（简单处理）
        path[path.size() - 1].curvature = path[path.size() - 3].curvature;
        path[path.size() - 2].curvature = path[path.size() - 3].curvature;
    }

    // 订阅全局路径
    rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr path_subscription_;
    // 订阅机器人位姿(base_link)
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odometry_subscription_;
    // 发布速度和角度
    rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_publisher_;
    // 控制器定时器，确定控制周期
    rclcpp::TimerBase::SharedPtr control_timer_;
    // 用于发布降采样后的路径
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr ds_path_publisher_;
    // 用于发布平滑后的路径
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr smooth_path_publisher_;

    // tf变换
    tf2_ros::Buffer            tf_buffer_;
    tf2_ros::TransformListener tf_listener_;

    // 路径跟随控制器
    ControllerInterface *controller_;
};